import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node

def generate_launch_description():

    # 获取机器人的share目录路径
    # bringup_dir = get_package_share_directory('robot_bringup')
    # config_dir = os.path.join(bringup_dir, 'config')

    # 参数文件路径
    # params_file = os.path.join(config_dir, 'shared_params.yaml')


    ################################################################################
    # 仿真世界（mower_description、hardware_module）
    ################################################################################
    mower_gazebo_dir = get_package_share_directory('mower_gazebo')
    mower_gazebo_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(mower_gazebo_dir,'launch','mower_world.launch.py'))
    )

    # 发布超声波数据
    ultrasonic_node_share_dir = get_package_share_directory('ultrasonic_node')
    ultrasonic_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(ultrasonic_node_share_dir,'launch','ultrasonic_node.launch.py')),
    )

    ################################################################################
    # nav_module
    ################################################################################

    # 传感器数据融合
    robot_localization_node_share_dir = get_package_share_directory('robot_localization_node')
    robot_localization_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(robot_localization_node_share_dir,'launch','robot_localization_node.launch.py')),
        # PythonLaunchDescriptionSource(os.path.join(robot_localization_node_share_dir,'launch','robot_localization_1.launch.py')),
   
    )

    ################################################################################
    # map_module
    ################################################################################

    # 地图模块
    # map_module_dir = get_package_share_directory('map_module')
    # map_module = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(os.path.join(map_module_dir,'launch','map_server.launch.py')),
    #     # launch_arguments={'params_file': os.path.join(map_module_dir, 'config', 'params.yaml')}.items(),
    #     # launch_arguments={'params_file': params_file}.items()
    # )

    ################################################################################
    # slam
    ################################################################################
    border_map_dir = get_package_share_directory('border_map')
    border_map_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(border_map_dir,'launch','border_map.launch.py')),
    )

    ################################################################################
    # 路径生成
    ################################################################################
    global_plan_path_dir = get_package_share_directory('global_plan_path')
    global_plan_path_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(global_plan_path_dir,'launch','global_plan_path.launch.py')),
    )

    ################################################################################
    # 运动控制
    ################################################################################
    dynamic_controller_dir = get_package_share_directory('dynamic_controller')
    dynamic_controller_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(dynamic_controller_dir,'launch','dynamic_controller.launch.py')),
    )

    ################################################################################
    # 面板
    ################################################################################
    
    panel_dir = get_package_share_directory('panel')
    panel_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(panel_dir,'launch','panel.launch.py')),
    )


    #-------------------------#

    # 创建LaunchDescription对象，并添加电机驱动节点和传感器launch文件的动作
    ld = LaunchDescription()
    ld.add_action(mower_gazebo_node)

    ld.add_action(ultrasonic_node) # 20250321

    ld.add_action(robot_localization_node)

    # ld.add_action(map_module)
    ld.add_action(border_map_node)

    ld.add_action(global_plan_path_node)
    # ld.add_action(dynamic_controller_node)
    ld.add_action(panel_node)
    
    return ld


